package com.robotic.test;

import lombok.Data;

import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.nio.charset.StandardCharsets;
import java.util.Arrays;

// For command 0x1008: Robot Run State Information
@Data
public class RcsData {
    private final String robotName;
    private final int currentMileage; // cm
    private final int totalMileage; // cm
    private final long currentRunTime; // s
    private final long totalRunTime; // s
    private final long currentMotionTime; // s
    private final long totalMotionTime; // s
    private final float[] joystick; // LX, LY, RX, RY
    private final byte[] rcsState;
    private final int errorState;
    public RcsData(byte[] data) {
        ByteBuffer buffer = ByteBuffer.wrap(data).order(ByteOrder.LITTLE_ENDIAN);
        byte[] nameBytes = new byte[15];
        buffer.get(nameBytes);
        this.robotName = new String(nameBytes, StandardCharsets.UTF_8).trim();
        buffer.get(); // Skip null terminator
        this.currentMileage = buffer.getInt();
        this.totalMileage = buffer.getInt();
        this.currentRunTime = buffer.getLong();
        this.totalRunTime = buffer.getLong();
        this.currentMotionTime = buffer.getLong();
        this.totalMotionTime = buffer.getLong();
        this.joystick = new float[4];
        for (int i = 0; i < 4; i++) {
            this.joystick[i] = buffer.getFloat();
        }
        this.rcsState = new byte[10];
        buffer.get(this.rcsState);
        buffer.get(new byte[2]); // Align
        this.errorState = buffer.getInt();
    }
    public boolean isNavMode() {
        return rcsState[0] == 1;
    }

    // Manual getter methods to resolve compilation issues
    public int getTotalMileage() {
        return totalMileage;
    }

    public String getErrorStateDescription() {
        StringBuilder errors = new StringBuilder();
        if ((errorState & 0b1) != 0) errors.append("IMU Error, ");
        if ((errorState & 0b10) != 0) errors.append("WiFi Error, ");
        if ((errorState & 0b100) != 0) errors.append("Driver Heat Warning, ");
        if ((errorState & 0b1000) != 0) errors.append("Driver Error, ");
        if ((errorState & 0b10000) != 0) errors.append("Motor Heat Warning, ");
        if ((errorState & 0b100000) != 0) errors.append("Battery Low Warning, ");
        if (errors.length() == 0) return "No Errors";
        return errors.substring(0, errors.length() - 2);
    }
    @Override
    public String toString() {
        return "RcsData{" +
                "robotName='" + robotName + '\'' +
                ", totalMileage=" + totalMileage + "cm" +
                ", totalRunTime=" + totalRunTime + "s" +
                ", isNavMode=" + isNavMode() +
                ", errorState=" + getErrorStateDescription() +
                '}';
    }
}
